/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.*;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
/**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     *  
     * 1: A
    2: B
    3: X
    4: Y
    5: Left Bumper
    6: Right Bumper
    7: Back
    8: Start
    9: Left Joystick
    10: Right Joystick

Axis Mappings #

The axis on the controller return values between -1 and 1, and follow the following mapping :

    1: Left Stick X Axis
        Left:Negative
        Right: Positive
    2: Left Stick Y Axis
        Up: Negative
        Down: Positive
    3: Triggers
        Left: Positive
        Right: Negative
    4: Right Stick X Axis
        Left: Negative
        Right: Positive
    5: Right Stick Y Axis
        Up: Negative
        Down: Positive
    6: Directional Pad (Not recommended, buggy)
     */
public class RobotTemplate extends IterativeRobot {
    
    //TODO All references are created but not instantiatecd here

    Talon leftMotor; //drive motors
    Talon rightMotor;
    RobotDrive driveMotors; //drive motor controller
    Jaguar climb1; //climbers
    Jaguar climb2; 
    Victor lifter; //lifter
    Victor pusher; //pusher
    Victor latch; //latch
    Victor dump;
    Encoder driveEncoder1;  //encoders
    Encoder driveEncoder2; 
    Joystick joystick1; //controls
    Joystick joystick2;
    Joystick xbox;
    Gyro gyro; //Gyro
    DigitalInput lifter1max;
    DigitalInput lifter1min;
    DigitalInput lifter2max;
    DigitalInput lifter2min;
    Relay lights;
    //Smartdashboard stuff
    SendableChooser autonomousChooser;
    SendableChooser values;
    
    
    //TODO Set these to the actual slot numbers
    private final int slotNumber = 2;
    private final int analogSlotNumber = 1;
    //Motor PWM Channels
    private final int driveMotor1PWM = 1;
    private final int driveMotor2PWM = 2;
    private final int climb1PWM = 3;
    private final int climb2PWM = 4;
    private final int lifterPWM = 5;
    private final int pusherPWM = 6;
    private final int latchPWM = 7;
    private final int dumpPWM = 8;
    
    //DIO Channels
    private final int lifter1maxPWM = 1; 
    private final int lifter2maxPWM = 2;
    private final int lifter1minPWM = 3;   
    private final int lifter2minPWM = 4;
    private final int encoder1Channel1 = 5;
    private final int encoder1Channel2 = 6;
    private final int encoder2Channel1 = 7;
    private final int encoder2Channel2 = 8;
    
    //Analog Channels
    private final int gyroChannel = 1;
    
    //Relay
    private final int lightsChannel = 1;
    
    //Intergers for driving autonomously
    private final double heading = 0.03;
    
    //teleop 
    double currentSpeed1 = 0;
    double currentSpeed2 = 0;
    double increment = .025;
    int lightVar = 0;
    


    
    //Object Choosers for Autnomous
    
    Object closePost;
    Object backPost;
    Object sidePost;
    Object noPost;
    Object selectedMode;
       
    Timer autonomousTimer;
    Timer latchTimer;
    
    
    
    
   
    

    
    //Example:  RobotDrive drive; 
    
    
    
    public void robotInit() {
        
//TODO Instantiate objects here
//Example: driveTrain = new RobotDrive(1,2);
        
    leftMotor = new Talon(driveMotor1PWM);
    rightMotor = new Talon(driveMotor2PWM);
    lifter = new Victor(lifterPWM); //lifter
    pusher = new Victor(pusherPWM);
    driveMotors = new RobotDrive(leftMotor, rightMotor);
    latch = new Victor(latchPWM);
    dump = new Victor(dumpPWM);
    
    climb1 = new Jaguar(climb1PWM);
    climb2 = new Jaguar(climb2PWM);
    
    lifter1max = new DigitalInput(lifter1maxPWM);
    lifter1min = new DigitalInput(lifter1minPWM);
    lifter2max = new DigitalInput(lifter2maxPWM);
    lifter2min = new DigitalInput(lifter2minPWM);

    lights = new Relay(lightsChannel);
    driveEncoder1 = new Encoder(encoder1Channel1,encoder1Channel2); 
    driveEncoder2 = new Encoder(encoder2Channel1,encoder2Channel2);
    driveEncoder1.start();
    driveEncoder2.start();
    gyro = new Gyro(analogSlotNumber, gyroChannel);

    joystick1 = new Joystick(1);
    joystick2= new Joystick(2);
    xbox = new Joystick(3);
    autonomousChooser = new SendableChooser();
    values = new SendableChooser();
    //speedMulti = new SendableChooser();
    closePost = new Object();
    backPost = new Object();
    sidePost = new Object();
    noPost = new Object();
   
   
    
    autonomousChooser.addObject("Closest post", closePost);
    autonomousChooser.addObject("Back post", backPost);
    autonomousChooser.addObject("Side post", sidePost);
    autonomousChooser.addDefault("DO NOTHING", noPost);
    SmartDashboard.putData("Autonomouse Program Chooser", autonomousChooser);
    
    
    
    //SmartDashboard.putData("Speed Multiplier", speedMulti);
    autonomousTimer = new Timer();
    latchTimer = new Timer();
    
    LiveWindow.addActuator("Drive", "Left Motor", leftMotor);
    LiveWindow.addActuator("Drive", "Right Motor", rightMotor);
//    LiveWindow.addActuator("Climb", "Climb 1", new Jaguar(3));
//    LiveWindow.addActuator("Climb", "Climb 2", new Jaguar(4));
//    LiveWindow.addActuator("Climb", "Lifter", new Jaguar(5));
    LiveWindow.addSensor("Drive", "Encoder 1", driveEncoder1);
    LiveWindow.addSensor("Drive", "Encoder 2", driveEncoder2);
    LiveWindow.addSensor("Drive", "Gyro", gyro);
    LiveWindow.addSensor("Lifter", "Lifter1Max", lifter1max);
    LiveWindow.addSensor("Lifter", "Lifter2Max", lifter2max);
    LiveWindow.addSensor("Lifter", "Lifter1Min", lifter1min);
    LiveWindow.addSensor("Lifter", "Lifter2Min", lifter2min);
    LiveWindow.addActuator("Lifter", "Lifter Motor", lifter);
    LiveWindow.addActuator("Pusher", "Pusher Motor", pusher);
    LiveWindow.addActuator("Lifter", "Latch Motor", latch);
    SmartDashboard.putNumber("Autonomous Delay", 1);
    
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousInit(){
    selectedMode = autonomousChooser.getSelected();
    gyro.reset();
    autonomousTimer.reset();
    autonomousTimer.start();
    }
    public void autonomousPeriodic() {

  
    
        if(autonomousTimer.get() >= SmartDashboard.getNumber("Autonomous Delay")){
       //close post 
       if(selectedMode.equals(closePost)){
       if(driveEncoder1.getDistance() < 10000 || -driveEncoder2.getDistance() < -10000){        
        
           driveMotors.drive(0.4, gyro.getAngle()*-heading);
       }
       }
       
       else if(selectedMode.equals(backPost)){
       if(-driveEncoder1.getDistance() < -10000 || driveEncoder2.getDistance() < 10000){        
        
           driveMotors.drive(-0.4, gyro.getAngle()*-heading);
       }
       }
        
       else  if(selectedMode.equals(sidePost)){
       if(driveEncoder1.getDistance() < 10000 || -driveEncoder2.getDistance() < -10000){        
        
           driveMotors.drive(0.4, gyro.getAngle()*-heading);
       }
       }

       
         
       else  if(selectedMode.equals(noPost)){
             driveMotors.drive(0,0);
         
         }
       
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
//  TODO Teleop Code
       // increment = speedMulti;
        SmartDashboard.putNumber("current speed left", currentSpeed1);
        SmartDashboard.putNumber("current speed right", currentSpeed2);
        SmartDashboard.putNumber("joy 1", joystick1.getRawAxis(1));
        SmartDashboard.putNumber("joy 2", joystick2.getRawAxis(1));
        double joystickValue1 = joystick1.getRawAxis(2);// Changed from 2 -- left stick
        double joystickValue2 = joystick2.getRawAxis(2);// Changed from 2 -- right stick
     //Drive--------------------------------------------------------------------Drive   
        //left side drive
        if (currentSpeed1<joystickValue1){
            currentSpeed1 = currentSpeed1 + increment;
        }
        else if (currentSpeed1>joystickValue1){
            currentSpeed1 = currentSpeed1 - increment;
        }
        //right side drive
        if (currentSpeed2<joystickValue2){
            currentSpeed2 = currentSpeed2 + increment;
        }
        else if (currentSpeed2>joystickValue2){
            currentSpeed2 = currentSpeed2 - increment;
        }
    driveMotors.tankDrive(-currentSpeed1, -currentSpeed2);   
    
    
    //latch---------------------------------------------------------------------Latch
    if(xbox.getRawButton(3)){ //Close Latch
       
       latch.set(0.5);

    }
    
    else if(xbox.getRawButton(2)){ 
        latch.set(-0.5);
       
    }
    else{
        latch.set(0);
    }
  
    //if(latchTimer.get() == 5)
    //{
    //    latch.set(Relay.Value.kOff);
    //    latchTimer.reset();
    //}
    //pusher--------------------------------------------------------------------Pusher
   // pusher.set(xbox.getRawAxis(5));
    if(xbox.getRawButton(8)){
        pusher.set(1.00);
    }
    else if(xbox.getRawButton(7)){
        pusher.set(-1.00);
    }
    else{
        pusher.set(0.0);
    }
   
    //lifter--------------------------------------------------------------------Lifter
   
  //  lifter.set(xbox.getRawAxis(2));
    if(xbox.getRawButton(4)){
        lifter.set(1.00);
    }
    else if(xbox.getRawButton(1)){
        lifter.set(-1.00);
    }
     else{
        lifter.set(0.0);
    }
   
   
    //climb---------------------------------------------------------------------Climb
    boolean overRide = joystick1.getRawAxis(3) > .6;
    if((((lifter1max.get() == true && xbox.getRawAxis(2) < 0)||(lifter1min.get() == true && xbox.getRawAxis(2) > 0))&&(Math.abs(xbox.getRawAxis(2))> 0.2))||(overRide)) {
       climb1.set(xbox.getRawAxis(2));
   }
   else {
       climb1.set(0);
   }
   if((((lifter2max.get() == true && xbox.getRawAxis(5) < 0)||(lifter2min.get() == true && xbox.getRawAxis(5) > 0))&&(Math.abs(xbox.getRawAxis(5))> 0.2)||(overRide))){
       climb2.set(-xbox.getRawAxis(5));
   }
   else {
       climb2.set(0);
   }
   //Dump-----------------------------------------------------------------------Dump
   if (joystick1.getRawButton(1)){
       dump.set(0.5);
   }
   else if(joystick2.getRawButton(1)){
       dump.set(-0.5);
   }
   else{
       dump.set(0.0);
   }
   
   
   //lights
    if (joystick1.getRawButton(10) == true){
        lightVar = 1 - lightVar;
    if (lightVar == 1){
        lights.set(Relay.Value.kOn);
    }
    else{
        lights.set(Relay.Value.kOff);
    }
    }    
    // if(xbox.getRawAxis(2) == 0){
   //     climb1.set(0);
   //     climb2.set(0);
   // }
   /* if((xbox.getRawAxis(3) > 0)){
        if((lifter1min.get() == false)){
        climb1.set(xbox.getRawAxis(3));    
        }
        else if((lifter1min.get() == true )){
        climb1.set(0);    
        }
        
    }
    if((xbox.getRawAxis(3) < 0)){
        if((lifter2min.get() == false)){
            climb2.set(xbox.getRawAxis(3));
        }
       else if((lifter2min.get() == true)){
            climb2.set(0);
        }
        
    }
    if((xbox.getRawButton(5))&&((xbox.getRawAxis(3) > 0))){
        if((lifter1max.get() == false)){
            climb1.set(-(xbox.getRawAxis(3)));
        }
        else if(lifter1max.get()){
            climb1.set(0);
            
        }
    }
    if((xbox.getRawButton(6))&&((xbox.getRawAxis(3) < 0))){
        if(lifter2max.get() == false){
         climb2.set(-(xbox.getRawAxis(3)));
        }
        else if(lifter2max.get()){
            climb2.set(0);
            
        }
        
    }
    */ 
    
// }       
    }
    public void testInit(){
   
        
    }
    public void testPeriodic(){
        LiveWindow.run();
        
    
    }
    
    
    
}
